
#include <memory.h>
#include "rgb_led.h"
#include "app_msgs.h"
#include "app_trace.h"
#include "recorder_task.h"
#include "indicate_task.h"
#include "fusion_task.h"

#include "prs_bmp280.h"
#include "app_config.h"
#include "app_sensor.h"
typedef struct {
  void (*error_handle)(const char *);
  imu_sensor_act_t *imu_sensor_act;
  bsp_msg_t *imu_msg_queue;
  bsp_msg_t *imu_recorder_queue;
} imu_task_para_t;

/**
 * imu中断服务函数
 * @param tm 系统时间
 * @param p 参数指针
 */
void imu_data_ready_callback(system_time_t *tm, void *p) {
    imu_msg_t msg;
    msg.tm = *tm;
    imu_task_para_t *task = (imu_task_para_t *) p;
    if(imu_read(task->imu_sensor_act, &msg.raw)){
        return;
    }
    bsp_msg_send(task->imu_msg_queue, &msg);
}

void error_handle(const char *s) {
    char *current_task_name = pcTaskGetName(NULL);
    forever_trace(current_task_name, "%s", s);
}

void imu_irq_init(imu_task_para_t *imu_task) {
    imu_sensor_act_t *imuact = imu_task->imu_sensor_act;
    imuact->imu_handle.trace = common_trace;
    imu_error_t err = imu_init(imuact);
    if (err == adis1646x_ok) {
        /*注册IMU的DR(data ready)中断服务函数*/
        register_gpio_irq(imuact->imu_handle.data_ready_pin, imu_data_ready_callback, (void *) imu_task);
    } else {
        error_handle("imu initial failed\n");
    }
}

#include <catlog.h>
_Noreturn void MainTask() {
    catlog_init();
    char *current_task_name = pcTaskGetName(NULL);
    common_trace_init();
    bsp_msg_t led_indicate_queue = {
        .queue = NULL,
        .isr_send = 0,
        .capacity = 1,
        .item_size = 1,
        .isr_get = 0
    };
    bsp_msg_init(&led_indicate_queue);
    led_task_para_t led_para = {
        .indicate_queue = &led_indicate_queue,
        .error_handle = error_handle,
        .rgb_led =  led_handle_create(),
    };
    bsp_task_t led_task = {
        .task_name = "led_task",
        .task_fn = led_indicate_task,
        .stack_size = 128,
        .priority=osPriorityIdle,
        .parameter = &led_para,
    };
    bsp_create_task(&led_task);

    /*初始化消息队列*/
    bsp_msg_t recorder_queue = {
        .queue = NULL,
        .item_size = sizeof(recorder_data_t),
        .capacity = 100,
        .isr_send = 0,
    };
    bsp_msg_init(&recorder_queue);
    bsp_msg_t imu_queue = {
        .queue = NULL,
        .item_size = sizeof(imu_msg_t),
        .capacity = 100,
        .isr_send = 1,
    };
    bsp_msg_init(&imu_queue);
#if APP_RUN_MODE == 0
    /*初始化日志任务*/
    recorder_task_para_t recorder_task_para = {
        .error_handler = error_handle,
        .recorder = NULL,
        .recorder_queue = &recorder_queue,
        .trace = common_trace,
    };
    bsp_task_t rec_task = {
        .task_name = "recorder task",
        .task_fn = recorder_task,
        .priority = osPriorityLow,
        .stack_size = 1024,
        .parameter = &recorder_task_para,
    };
    bsp_create_task(&rec_task);
#endif
    /** 初始化IMU中断*/
    imu_task_para_t imu_task_para = {
        .imu_sensor_act = &imu_sensor_act,
        .error_handle = error_handle,
        .imu_msg_queue = &imu_queue,
        .imu_recorder_queue = NULL,
    };
    imu_irq_init(&imu_task_para);

    /** 初始化气压计 **/
    bmp280_handle_t bmp280_dev = {
        .dev = &spi4_dev,
        .trace = common_trace,
        .delay_ms = bsp_delay_ms
    };
//    bmp280_init(&bmp280_dev);

#if APP_DEBUG_ENABLE == 0
    imu_raw_t raw;
    do{
        imu_sensor_act.imu_read(&imu_sensor_act.imu_handle,&raw);
        common_trace("imu: temp= %d acc = %d %d %d\n",
                     (int)((float)raw.temp/326.8f+25),
                     (raw.acce[0]),
                     (raw.acce[1]),
                     (raw.acce[2])
                     );
        bsp_delay_ms(200);
    }while(1);
#endif

#if APP_RUN_MODE == 1
    /** 初始化组合导航任务 */
    FusionOption_t *opt = bsp_malloc(sizeof(FusionOption_t));
    opt->delta_t = imu_task_para.imu_sensor_act->imu_handle.attrib->delta_t;
    opt->acce_scale = imu_task_para.imu_sensor_act->imu_handle.attrib->acce_scale;
    opt->gyro_scale = imu_task_para.imu_sensor_act->imu_handle.attrib->gyro_scale;
    fusion_task_para_t fusion_para = {
        .trace = common_trace,
        .error_handler = error_handle,
        .recorder_que = &recorder_queue,
        .imu_msg_que = &imu_queue,
        .indicate_que = &led_indicate_queue,
        .option = opt,
        .rst_dev = &uart1_dev,
    };
    bsp_task_t fusion_handle = {
        .task_name = "fusion task",
        .parameter = &fusion_para,
        .stack_size = 2048 * 5,
        .task_fn = fusion_task,
        .priority = osPriorityHigh,
    };
    bsp_create_task(&fusion_handle);
#endif
    while (1) {
//        common_trace(" the min free stack size is %d \r\n",(int32_t)uxTaskGetStackHighWaterMark(fusion_handle.handle));
        osDelay(5000);
    }

}

